using BepuPhysics.Collidables;
using BepuUtilities.Collections;
using BepuUtilities.Memory;
using System.Diagnostics;
using System.Runtime.CompilerServices;

namespace BepuPhysics.CollisionDetection
{
    partial class PairCache
    {
        internal struct CollisionPairLocation
        {
            public CollidablePair Pair;
            // 仅当碰撞对移动到休眠集中时使用。
            public int InactiveSetIndex;
            public int InactivePairIndex;
        }

        /// <summary>
        /// 从约束句柄映射回冲突检测对缓存位置。
        /// </summary>
        internal Buffer<CollisionPairLocation> ConstraintHandleToPair;


        // 此缓冲区与Bodies.Sets和Solver.Sets并行填充。
        // 请注意,这不包括活动集,因此索引0始终为空。
        internal Buffer<SleepingSet> SleepingSets;

        internal void ResizeSetsCapacity(int setsCapacity, int potentiallyAllocatedCount)
        {
            Debug.Assert(setsCapacity >= potentiallyAllocatedCount && potentiallyAllocatedCount <= SleepingSets.Length);
            setsCapacity = BufferPool.GetCapacityForCount<SleepingSet>(setsCapacity);
            if (SleepingSets.Length != setsCapacity)
            {
                var oldCapacity = SleepingSets.Length;
                pool.ResizeToAtLeast(ref SleepingSets, setsCapacity, potentiallyAllocatedCount);
                if (oldCapacity < SleepingSets.Length)
                    SleepingSets.Clear(oldCapacity, SleepingSets.Length - oldCapacity); // 我们依赖于默认初始化未使用的插槽。
            }
        }

        [Conditional("DEBUG")]
        internal unsafe void ValidateConstraintHandleToPairMapping()
        {
            ValidateConstraintHandleToPairMapping(ref workerCaches, false);
        }
        [Conditional("DEBUG")]
        internal unsafe void ValidateConstraintHandleToPairMappingInProgress(bool ignoreStale)
        {
            ValidateConstraintHandleToPairMapping(ref NextWorkerCaches, ignoreStale);
        }

        [Conditional("DEBUG")]
        internal unsafe void ValidateConstraintHandleToPairMapping(ref ArrayList<WorkerPairCache> caches, bool ignoreStale)
        {
            for (int i = 0; i < Mapping.Count; ++i)
            {
                if (!ignoreStale || PairFreshness[i] > 0)
                {
                    var existingCache = Mapping.Values[i].ConstraintCache;
                    var existingHandle = *(int*)(caches[existingCache.Cache].constraintCaches[existingCache.Type].Buffer.Memory + existingCache.Index);
                    Debug.Assert(existingCache.Active, "The overlap mapping should only contain references to constraints which are active.");
                    ref var pairLocation = ref ConstraintHandleToPair[existingHandle];
                    Debug.Assert(new CollidablePairComparer().Equals(ref ConstraintHandleToPair[existingHandle].Pair, ref Mapping.Keys[i]),
                        "The overlap mapping and handle mapping should match.");
                }
            }
        }

        [Conditional("DEBUG")]
        internal unsafe void ValidateHandleCountInMapping(ConstraintHandle constraintHandle, int expectedCount)
        {
            int count = 0;
            for (int i = 0; i < Mapping.Count; ++i)
            {
                var existingCache = Mapping.Values[i].ConstraintCache;
                var existingHandle = *(int*)(workerCaches[existingCache.Cache].constraintCaches[existingCache.Type].Buffer.Memory + existingCache.Index);
                if (existingHandle == constraintHandle.Value)
                {
                    ++count;
                    Debug.Assert(count <= expectedCount && count <= 1, "Expected count violated.");
                }
            }
            Debug.Assert(count == expectedCount, "Expected count for this handle not found!");
        }

        internal unsafe void SleepTypeBatchPairs(ref SleepingSetBuilder builder, int setIndex, Solver solver)
        {
            ref var constraintSet = ref solver.Sets[setIndex];
            for (int batchIndex = 0; batchIndex < constraintSet.Batches.Count; ++batchIndex)
            {
                ref var batch = ref constraintSet.Batches[batchIndex];
                for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex)
                {
                    ref var typeBatch = ref batch.TypeBatches[typeBatchIndex];
                    Debug.Assert(typeBatch.ConstraintCount > 0, "If a type batch exists, it should contain constraints.");
                    if (NarrowPhase.IsContactConstraintType(typeBatch.TypeId))
                    {
                        for (int indexInTypeBatch = 0; indexInTypeBatch < typeBatch.ConstraintCount; ++indexInTypeBatch)
                        {
                            var handle = typeBatch.IndexToHandle[indexInTypeBatch];
                            ref var pairLocation = ref ConstraintHandleToPair[handle.Value];
                            Mapping.GetTableIndices(ref pairLocation.Pair, out var tableIndex, out var elementIndex);
                            ref var cacheLocations = ref Mapping.Values[elementIndex];
                            Debug.Assert(cacheLocations.ConstraintCache.Exists);

                            pairLocation.InactiveSetIndex = setIndex;
                            pairLocation.InactivePairIndex = builder.Add(ref workerCaches, pool, ref Mapping.Keys[elementIndex], ref cacheLocations);

                            // 现在,任何现有缓存数据都已移入非活动集,我们应该从重叠映射中删除重叠。
                            Mapping.FastRemove(tableIndex, elementIndex);
                        }
                    }
                }
            }
            builder.FinalizeSet(pool, out SleepingSets[setIndex]);
        }

        internal ref WorkerPairCache GetCacheForAwakening()
        {
            // 注意,SET的目标位置取决于唤醒是否从窄阶段的上下文中执行。
            // 无论哪种方式,我们都需要将数据放入最近更新的缓存中。如果这发生在狭窄阶段(即NextWorkerCache)内,
            // 因为我们还没有翻转缓冲器。如果它在窄阶段之外,那么它就是当前的workerCache。
            // 我们可以通过检查是否分配了NextWorkerCache来区分这两者。它们不存在于狭窄阶段的执行之外。

            // 还要注意,我们只处理一个工作缓存。唤醒只是将新的缓存转储到第一线程中。这件事解决了,因为
            // 目前,实际的对缓存修改是本地顺序的。
            if (NextWorkerCaches.Allocated && NextWorkerCaches.Count > 0 && NextWorkerCaches[0].collisionCaches.Allocated)
                return ref NextWorkerCaches[0];
            if (workerCaches.Allocated)
                return ref workerCaches[0];
            // 尚不存在缓存;这必须是在第一次更新之前发生的外部调用。懒惰地初始化一个工作缓存。
            workerCaches = new ArrayList<WorkerPairCache>(1);
            var constraints = new QuickList<WorkerPairCache.PreallocationSizes>(1, pool);
            var collisions = new QuickList<WorkerPairCache.PreallocationSizes>(1, pool);
            workerCaches.AllocateUnsafely() = new WorkerPairCache(0, pool, ref constraints, ref collisions, 0);
            constraints.Dispose(pool);
            collisions.Dispose(pool);
            return ref workerCaches[0];
        }

        private unsafe PairCacheIndex CopyCacheForAwakening(ref Buffer<SleepingCache> inactiveCaches, ref Buffer<UntypedList> activeCaches, TypedIndex sourceCacheIndex)
        {
            ref var sourceCache = ref inactiveCaches[sourceCacheIndex.Type];
            // 请注意,sourceCacheIndex.Type指的是压缩列表中类型的索引,而不是不连续的类型id。
            // 未打包的活动缓存使用不连续的类型ID,因此现在引用的是sourceCache.TypeId,而不是sourceCacheIndex.Type。
            ref var targetCache = ref activeCaches[sourceCache.TypeId];
            var targetByteIndex = targetCache.Allocate(sourceCache.List.ElementSizeInBytes, sourceCache.List.Count, pool);
            Unsafe.CopyBlockUnaligned(targetCache.Buffer.Memory + targetByteIndex, sourceCache.List.Buffer.Memory + sourceCacheIndex.Index, (uint)sourceCache.List.ElementSizeInBytes);
            // 请注意,为激活的条目选择的缓存始终是第一个缓存,因此缓存索引仅为0。
            return new PairCacheIndex(0, sourceCache.TypeId, targetByteIndex);
        }
        internal unsafe void AwakenSet(int setIndex)
        {
            ref var sleepingSet = ref SleepingSets[setIndex];
            // 如果没有对,则不需要非活动集,因此不能保证分配。
            if (sleepingSet.Allocated)
            {
                ref var activeSet = ref GetCacheForAwakening();
                // 为简单起见,唤醒只是遍历休眠集合中的配对列表。
                // 通过构造非活动集,缓存访问将是高度一致的缓存,因此它不执行批量复制这一事实并不是那么糟糕。
                // (我们可以让它进行批量复制,但只有在有任何理由的情况下才会这么做。)
                for (int i = 0; i < sleepingSet.Pairs.Count; ++i)
                {
                    ref var pair = ref sleepingSet.Pairs[i];
                    CollidablePairPointers pointers;
                    pointers.ConstraintCache = CopyCacheForAwakening(ref sleepingSet.ConstraintCaches, ref activeSet.constraintCaches, pair.ConstraintCache);
                    if (pair.CollisionCache.Exists)
                    {
                        pointers.CollisionDetectionCache = CopyCacheForAwakening(ref sleepingSet.CollisionCaches, ref activeSet.collisionCaches, pair.CollisionCache);
                    }
                    else
                    {
                        pointers.CollisionDetectionCache = new PairCacheIndex();
                    }
                    Mapping.AddUnsafelyRef(ref pair.Pair, pointers);
                }
            }
        }

        internal void RemoveReferenceIfContactConstraint(ConstraintHandle handle, int typeId)
        {
            if (NarrowPhase.IsContactConstraintType(typeId))
            {
                var removed = Mapping.FastRemoveRef(ref ConstraintHandleToPair[handle.Value].Pair);
                Debug.Assert(removed, "If a contact constraint is being directly removed, it must exist within the pair mapping- " +
                    "all *active* contact constraints do, and it's not valid to attempt to remove an inactive constraint.");
            }
        }
    }
}
